﻿using DevExpress.Xpo.DB.Helpers;
using HalconDotNet;
using ImageDefine;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.Serialization.Formatters.Binary;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace ImageCalibModule
{
    [Serializable]
    public class ImageCalibHandeye:ImageCalibBase
    {
        public double[] dImageX;
        public double[] dImageY;
        public double[] dTableX;
        public double[] dTableY;


        public double[] dImageAngleX;
        public double[] dImageAngleY;
        public double[] dTableAngleX;
        public double[] dTableAngleY;
        public double[] dTableAngle;

        public double[] dImageXCalib;
        public double[] dImageYCalib;
        public double[] dTableXCalib;
        public double[] dTableYCalib;

        public double dPitchX;
        public double dPitchY;
        public bool HandEyeCalib { get; set; }

        public double dExporsureMs { get; set; }
        public int iGain { get; set; }

        public double Pitch { get; set; }

        public double ZOffSet { get; set; }

        public double FixHeight { get; set; }

        public double ForceOffZ { get; set; }

        [NonSerialized]
        public string strSettingPath;
        public ImageCalibHandeye()
        {
            strName = "";
            strRemark = "";
            
            dImageX = new double[9];
            dImageY = new double[9];
            dTableX = new double[9];
            dTableY = new double[9];

            dImageXCalib = new double[9];
            dImageYCalib = new double[9];
            dTableXCalib = new double[9];
            dTableYCalib = new double[9];

            dImageAngleX = new double[2];
            dImageAngleY = new double[2];
            dTableAngleX = new double[2];
            dTableAngleY = new double[2];
            dTableAngle = new double[2];

            HandEyeCalib = true;
            for (int i = 0; i < 9; i++)
            {
                //strPosName[i] = "";
                dImageX[i] = 0.0;
                dImageY[i] = 0.0;
                dTableX[i] = 0.0;
                dTableY[i] = 0.0;
                dImageXCalib[i] = 0.0;
                dImageYCalib[i] = 0.0;
                dTableXCalib[i] = 0.0;
                dTableYCalib[i] = 0.0;
            }


            CalibPara = new ImageCalibParameter();
        }
        public static ImageCalibHandeye Load(string strPath,string strName)
        {
            ImageCalibHandeye pDoc = null;
            BinaryFormatter binSerializer = new BinaryFormatter();
            FileStream fsReader = null;
            try
            {
                fsReader = File.OpenRead(strPath + @"/ImageCalibHandeye_" +strName+ ".bat");
                pDoc = (ImageCalibHandeye)binSerializer.Deserialize(fsReader);
                fsReader.Close();
                pDoc.strSettingPath = strPath;
                pDoc.strName = strName;
                
            }
            catch (IOException)
            {
                if (fsReader != null)
                {
                    fsReader.Close();
                }
                pDoc = new ImageCalibHandeye();
                pDoc.strSettingPath = strPath;
                pDoc.strName = strName;
                pDoc.Save();
            }
            catch (Exception)
            {
                if (fsReader != null)
                {
                    fsReader.Close();
                }
                pDoc = new ImageCalibHandeye();
                pDoc.strSettingPath = strPath;
                pDoc.strName = strName;
                pDoc.Save();
            }
            return pDoc;
        }
        public bool Save()
        {
            if (!Directory.Exists(strSettingPath))
            {
                Directory.CreateDirectory(strSettingPath);
            }
            FileStream fsWriter = new FileStream(strSettingPath + @"/ImageCalibHandeye_" + strName + ".bat", FileMode.Create, FileAccess.Write, FileShare.Read);

            BinaryFormatter binSerializer = new BinaryFormatter();
            binSerializer.Serialize(fsWriter, this);
            fsWriter.Flush(true);
            fsWriter.Close();
            return true;
        }

        override public bool TramImageToWorld(double dImageX, double dImageY, double dWorldX, double dWorldY, out double dResultX, out double dResultY)
        {
            HTuple cameraParam = new HTuple();
            HTuple finalPoseTemp = new HTuple();

            cameraParam.Append(CalibPara.ResultFocus);
            cameraParam.Append(CalibPara.ResultKappa);
            cameraParam.Append(CalibPara.ResultSx);
            cameraParam.Append(CalibPara.ResultSy);
            cameraParam.Append(CalibPara.ResultCx);
            cameraParam.Append(CalibPara.ResultCy);
            cameraParam.Append(CalibPara.ResultImageWidth);
            cameraParam.Append(CalibPara.ResultImageHeight);

            finalPoseTemp.Append(CalibPara.FianlPosX);
            finalPoseTemp.Append(CalibPara.FianlPosY);
            finalPoseTemp.Append(CalibPara.FianlPosZ);
            finalPoseTemp.Append(CalibPara.FianlPosU);
            finalPoseTemp.Append(CalibPara.FianlPosV);
            finalPoseTemp.Append(CalibPara.FianlPosW);
            finalPoseTemp.Append(0);
            ForceOffZ = CalibPara.FianlPosZ * ZOffSet / FixHeight;
            HTuple finalPose;
            HOperatorSet.SetOriginPose(finalPoseTemp, 0, 0, ForceOffZ, out finalPose);
            try
            {
                HTuple resultX; HTuple resultY;
                HOperatorSet.ImagePointsToWorldPlane(cameraParam, finalPose, dImageY, dImageX, 1.0, out resultX, out resultY);
                if (CalibPara.CamPos == CamPos.XY)
                {
                    dResultX = dWorldX + resultX;
                    dResultY = dWorldY + resultY;
                }
                else
                {
                    //dResultX = resultX;
                    //dResultY = resultY;
                    dResultX = dTableXCalib[4] + resultX;
                    dResultY = dTableYCalib[4] + resultY;
                }
                return true;
            }
            catch
            {
                dResultX = dWorldX;
                dResultY = dWorldY;
                return true;
            }
        }
        override public bool TramImageToWorld(double dImageX, double dImageY, double dImageA, double dWorldX, double dWorldY, out double dResultX, out double dResultY, out double dResultA)
        {
            HTuple cameraParam = new HTuple();
            HTuple finalPoseTemp = new HTuple();

            cameraParam.Append(CalibPara.ResultFocus);
            cameraParam.Append(CalibPara.ResultKappa);
            cameraParam.Append(CalibPara.ResultSx);
            cameraParam.Append(CalibPara.ResultSy);
            cameraParam.Append(CalibPara.ResultCx);
            cameraParam.Append(CalibPara.ResultCy);
            cameraParam.Append(CalibPara.ResultImageWidth);
            cameraParam.Append(CalibPara.ResultImageHeight);

            finalPoseTemp.Append(CalibPara.FianlPosX);
            finalPoseTemp.Append(CalibPara.FianlPosY);
            finalPoseTemp.Append(CalibPara.FianlPosZ);
            finalPoseTemp.Append(CalibPara.FianlPosU);
            finalPoseTemp.Append(CalibPara.FianlPosV);
            finalPoseTemp.Append(CalibPara.FianlPosW);
            finalPoseTemp.Append(0);
            HTuple finalPose;
            ForceOffZ = CalibPara.FianlPosZ * ZOffSet / FixHeight;
            HOperatorSet.SetOriginPose(finalPoseTemp, 0, 0, ForceOffZ, out finalPose);

            try
            {
                HTuple resultX; HTuple resultY;
                HOperatorSet.ImagePointsToWorldPlane(cameraParam, finalPose, dImageY, dImageX, 1.0, out resultX, out resultY);
                if (CalibPara.CamPos == CamPos.XY)
                {
                    dResultX = dWorldX + resultX;
                    dResultY = dWorldY + resultY;
                }
                else
                {
                    //dResultX = resultX;
                    //dResultY = resultY;
                    dResultX = dTableXCalib[4] + resultX;
                    dResultY = dTableYCalib[4] + resultY;


                }
                dResultA = dImageA + CalibPara.FianlPosU;
                return true;
            }
            catch
            {
                dResultX = dWorldX;
                dResultY = dWorldY;
                dResultA = dImageA + CalibPara.FianlPosU;
                return true;
            }
        }
        public void SetZOffSet(double dNewZoffSet)
        {

            ZOffSet = dNewZoffSet;

            ForceOffZ = CalibPara.ResultFocus * ZOffSet / FixHeight;
        }
        public bool CalibCamWithData(double[] dImageXInput, double[] dImageYInput, double[] dTableXInput, double[] dTableYInput)
        {
            if (dImageXInput.Length != 9)
            {
                return false;
            }
            for (int i = 0; i < 9; i++)
            {
                dImageX[i] = dImageXInput[i];
                dImageY[i] = dImageYInput[i];
                dTableX[i] = dTableXInput[i];
                dTableY[i] = dTableYInput[i];
            }//9点赋值
            SortingPos();//数据排列
            if (CalibActioner() == false)//第一次标定
                return false;

            return true;
        }
        public bool CalibCamHandeye(double[] dImageXInput, double[] dImageYInput, double[] dTableXInput, double[] dTableYInput,
            double[] dImageXAngleInput, double[] dImageYAngleInput, double[] dTableXAngleInput, double[] dTableYAngleInput, double[] dTableAngleInput)
        {
            if (dImageXInput.Length != 9)
            {
                return false;
            }
            if (dImageXAngleInput.Length != 2)
            {
                return false;
            }
            for (int i = 0; i < 9; i++)
            {
                dImageX[i] = dImageXInput[i];
                dImageY[i] = dImageYInput[i];
                dTableX[i] = dTableXInput[i];
                dTableY[i] = dTableYInput[i];
            }//9点赋值
            SortingPos();//数据排列
            if (CalibActioner() == false)//第一次标定
                return false;
            for (int i = 0; i < 2; i++)
            {
                dImageAngleX[i] = dImageXAngleInput[i];
                dImageAngleY[i] = dImageYAngleInput[i];
                dTableAngleX[i] = dTableXAngleInput[i];
                dTableAngleY[i] = dTableYAngleInput[i];
                dTableAngle[i] = dTableAngleInput[i];
            }

            CalibActionerWithHandEyeDate();
            return true;
        }
        public bool CalibCamSelf()
        {
           
            SortingPos();//数据排列
            if (CalibActioner() == false)//第一次标定
                return false;
            CalibActionerWithHandEyeDate();
            return true;
        }
        public bool CalibCamSelfWithOutAngle()
        {

            SortingPos();//数据排列
            if (CalibActioner() == false)//第一次标定
                return false;
           
            return true;
        }
        private void SortingPos()
        {

            
            int[] RowOrderArrange = new int[9];
            ImageAndTableItem midImageAndTable = new ImageAndTableItem();
            List<ImageAndTableItem> imageTableItems = new List<ImageAndTableItem>();
            for (int i = 0; i < dImageY.Length; i++)
            {
                ImageAndTableItem newItem = new ImageAndTableItem();
                newItem.ImageX = dImageX[i];
                newItem.ImageY = dImageY[i];
                newItem.TableX = dTableX[i];
                newItem.TableY = dTableY[i];
                imageTableItems.Add(newItem);
            }
            #region 按Y排序

            for (int i = 0; i < imageTableItems.Count; i++)
            {

                for (int j = i + 1; j < imageTableItems.Count; j++)
                {
                    if (imageTableItems[i].ImageY > imageTableItems[j].ImageY)
                    {
                        ImageAndTableItem newItem = new ImageAndTableItem();
                        newItem.ImageX = imageTableItems[i].ImageX;
                        newItem.ImageY = imageTableItems[i].ImageY;
                        newItem.TableX = imageTableItems[i].TableX;
                        newItem.TableY = imageTableItems[i].TableY;

                        imageTableItems[i].ImageX = imageTableItems[j].ImageX;
                        imageTableItems[i].ImageY = imageTableItems[j].ImageY;
                        imageTableItems[i].TableX = imageTableItems[j].TableX;
                        imageTableItems[i].TableY = imageTableItems[j].TableY;

                        imageTableItems[j].ImageX = newItem.ImageX;
                        imageTableItems[j].ImageY = newItem.ImageY;
                        imageTableItems[j].TableX = newItem.TableX;
                        imageTableItems[j].TableY = newItem.TableY;

                    }
                }
            }


            #endregion
            #region 按X排序
            for (int i = 0; i < 3; i++)
            {
                for (int j = i + 1; j < 3; j++)
                {
                    if (imageTableItems[i].ImageX > imageTableItems[j].ImageX)
                    {
                        ImageAndTableItem newItem = new ImageAndTableItem();
                        newItem.ImageX = imageTableItems[i].ImageX;
                        newItem.ImageY = imageTableItems[i].ImageY;
                        newItem.TableX = imageTableItems[i].TableX;
                        newItem.TableY = imageTableItems[i].TableY;

                        imageTableItems[i].ImageX = imageTableItems[j].ImageX;
                        imageTableItems[i].ImageY = imageTableItems[j].ImageY;
                        imageTableItems[i].TableX = imageTableItems[j].TableX;
                        imageTableItems[i].TableY = imageTableItems[j].TableY;

                        imageTableItems[j].ImageX = newItem.ImageX;
                        imageTableItems[j].ImageY = newItem.ImageY;
                        imageTableItems[j].TableX = newItem.TableX;
                        imageTableItems[j].TableY = newItem.TableY;

                    }
                }
            }
            for (int i = 3; i < 6; i++)
            {
                for (int j = i + 1; j < 6; j++)
                {
                    if (imageTableItems[i].ImageX > imageTableItems[j].ImageX)
                    {
                        ImageAndTableItem newItem = new ImageAndTableItem();
                        newItem.ImageX = imageTableItems[i].ImageX;
                        newItem.ImageY = imageTableItems[i].ImageY;
                        newItem.TableX = imageTableItems[i].TableX;
                        newItem.TableY = imageTableItems[i].TableY;

                        imageTableItems[i].ImageX = imageTableItems[j].ImageX;
                        imageTableItems[i].ImageY = imageTableItems[j].ImageY;
                        imageTableItems[i].TableX = imageTableItems[j].TableX;
                        imageTableItems[i].TableY = imageTableItems[j].TableY;

                        imageTableItems[j].ImageX = newItem.ImageX;
                        imageTableItems[j].ImageY = newItem.ImageY;
                        imageTableItems[j].TableX = newItem.TableX;
                        imageTableItems[j].TableY = newItem.TableY;

                    }
                }

            }
            for (int i = 6; i < 9; i++)
            {
                for (int j = i + 1; j < 9; j++)
                {
                    if (imageTableItems[i].ImageX > imageTableItems[j].ImageX)
                    {
                        ImageAndTableItem newItem = new ImageAndTableItem();
                        newItem.ImageX = imageTableItems[i].ImageX;
                        newItem.ImageY = imageTableItems[i].ImageY;
                        newItem.TableX = imageTableItems[i].TableX;
                        newItem.TableY = imageTableItems[i].TableY;

                        imageTableItems[i].ImageX = imageTableItems[j].ImageX;
                        imageTableItems[i].ImageY = imageTableItems[j].ImageY;
                        imageTableItems[i].TableX = imageTableItems[j].TableX;
                        imageTableItems[i].TableY = imageTableItems[j].TableY;

                        imageTableItems[j].ImageX = newItem.ImageX;
                        imageTableItems[j].ImageY = newItem.ImageY;
                        imageTableItems[j].TableX = newItem.TableX;
                        imageTableItems[j].TableY = newItem.TableY;

                    }
                }

            }
            #endregion
            #region 赋值
            for (int i = 0; i < 9; i++)
            {
                dImageXCalib[i] = imageTableItems[i].ImageX;
                dImageYCalib[i] = imageTableItems[i].ImageY;
                dTableXCalib[i] = imageTableItems[i].TableX;
                dTableYCalib[i] = imageTableItems[i].TableY;

            }

            #endregion
        }

        private bool CalibActioner()
        {
            //HPose.CameraCalibration(HTuple NX, HTuple NY, HTuple NZ, HTuple NRow, HTuple NCol, HTuple startCamParam, HPose[] NStartPose, HTuple estimateParams, out HPose[] NFinalPose, out HTuple errors)
            HTuple NX; HTuple NY; HTuple NZ; HTuple NRow; HTuple NCol;
            NX = new HTuple();
            NY = new HTuple();
            NZ = new HTuple();
            NRow = new HTuple();
            NCol = new HTuple();
            double[] difX = new double[9];
            double[] difY = new double[9];
            double dPith = Math.Abs((dTableXCalib[0] - dTableXCalib[4]) / 1000.0);
            int i = 0;
            difX[i] = -dPith;
            difY[i] = -dPith;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 1;
            difX[i] = 0;
            difY[i] = -dPith;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 2;
            difX[i] = dPith;
            difY[i] = -dPith; ;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 3;
            difX[i] = -dPith;
            difY[i] = 0;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 4;
            difX[i] = 0;
            difY[i] = 0;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 5;
            difX[i] = dPith;
            difY[i] = 0;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 6;
            difX[i] = -dPith;
            difY[i] = dPith;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 7;
            difX[i] = 0;
            difY[i] = dPith;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);

            i = 8;
            difX[i] = dPith;
            difY[i] = dPith;
            NX.Append(difX[i]);
            NY.Append(difY[i]);
            NZ.Append(0.0);
            NRow.Append(dImageYCalib[i]);
            NCol.Append(dImageXCalib[i]);


            HTuple startCamParam = new HTuple();
            startCamParam.Append(CalibPara.StartFocus);
            startCamParam.Append(CalibPara.StartKappa);
            startCamParam.Append(CalibPara.StartSx);
            startCamParam.Append(CalibPara.StartSy);
            startCamParam.Append(CalibPara.StartCx);
            startCamParam.Append(CalibPara.StartCy);
            startCamParam.Append(CalibPara.StartImageWidth);
            startCamParam.Append(CalibPara.StartImageHeight);

            //
            HTuple NStartPose = new HTuple();
            NStartPose.Append(0.0);
            NStartPose.Append(0.0);
            NStartPose.Append(CalibPara.StartFocus);
            NStartPose.Append(0.0);
            NStartPose.Append(0.0);
            NStartPose.Append(0.0);
            NStartPose.Append(0);

            HTuple estimateParams = new HTuple("camera");
            HTuple cameraParam; HTuple NFinalPose; HTuple errors;
            try
            {
                HOperatorSet.CameraCalibration(NX, NY, NZ, NRow, NCol, startCamParam, NStartPose, estimateParams,
                    out cameraParam, out NFinalPose, out errors);
                CalibPara.FianlError = errors.D;
                //cameraParam[0].D = Math.Abs(cameraParam[0].D);
                CalibPara.ResultFocus = cameraParam[1].D;
                CalibPara.ResultKappa = cameraParam[2].D;
                CalibPara.ResultSx = cameraParam[3].D;
                CalibPara.ResultSy = cameraParam[4].D;
                CalibPara.ResultCx = cameraParam[5].D;
                CalibPara.ResultCy = cameraParam[6].D;
                CalibPara.ResultImageWidth = cameraParam[7].I;
                CalibPara.ResultImageHeight = cameraParam[8].I;

                //MessageBox.Show(curCalibItem.calibPara.ResultFocus.ToString());
                //HOperatorSet.VectorToPose(HTuple worldX, HTuple worldY, HTuple worldZ, HTuple imageRow, HTuple imageColumn, HTuple cameraParam, HTuple method, HTuple qualityType, out HTuple pose, out HTuple quality)
                HTuple worldX = new HTuple();
                HTuple worldY = new HTuple();
                HTuple worldZ = new HTuple();
                HTuple imageRow = new HTuple();
                HTuple imageColumn = new HTuple();

                if (CalibPara.CamPos == CamPos.XY)
                {
                    worldX.Append(0.0);
                    worldY.Append(0.0);
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[1]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[1]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[5]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[5]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[7]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[7]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[3]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[3]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[0]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[0]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[2]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[2]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[6]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[6]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[4] - dTableXCalib[8]));
                    worldY.Append((dTableYCalib[4] - dTableYCalib[8]));
                    worldZ.Append(0.0);
                }
                else
                {
                    worldX.Append(0.0);
                    worldY.Append(0.0);
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[1] -dTableXCalib[4]));
                    worldY.Append((dTableYCalib[1] -dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[5] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[5] - dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[7] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[7] - dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[3] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[3] - dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[0] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[0] - dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[2] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[2] - dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[6] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[6] - dTableYCalib[4]));
                    worldZ.Append(0.0);

                    worldX.Append((dTableXCalib[8] - dTableXCalib[4]));
                    worldY.Append((dTableYCalib[8] - dTableYCalib[4]));
                    worldZ.Append(0.0);
                }

                imageRow.Append(dImageYCalib[4]);
                imageColumn.Append(dImageXCalib[4]);

                imageRow.Append(dImageYCalib[1]);
                imageColumn.Append(dImageXCalib[1]);

                imageRow.Append(dImageYCalib[5]);
                imageColumn.Append(dImageXCalib[5]);

                imageRow.Append(dImageYCalib[7]);
                imageColumn.Append(dImageXCalib[7]);

                imageRow.Append(dImageYCalib[3]);
                imageColumn.Append(dImageXCalib[3]);

                imageRow.Append(dImageYCalib[0]);
                imageColumn.Append(dImageXCalib[0]);

                imageRow.Append(dImageYCalib[2]);
                imageColumn.Append(dImageXCalib[2]);

                imageRow.Append(dImageYCalib[6]);
                imageColumn.Append(dImageXCalib[6]);

                imageRow.Append(dImageYCalib[8]);
                imageColumn.Append(dImageXCalib[8]);

                HTuple finalPose;
                HTuple quality;
                HTuple finalPoseF;
                HOperatorSet.VectorToPose(worldX, worldY, worldZ, imageRow, imageColumn, cameraParam, "iterative", "error", out finalPose, out quality);


                CalibPara.FianlPosX = finalPose[0].D;
                CalibPara.FianlPosY = finalPose[1].D;
                CalibPara.FianlPosZ = finalPose[2].D;
                CalibPara.FianlPosU = finalPose[3].D;
                CalibPara.FianlPosV = finalPose[4].D;
                CalibPara.FianlPosW = finalPose[5].D;
                HTuple resultX; HTuple resultY;

                //HOperatorSet.ImagePointsToWorldPlane(cameraParam, finalPose, curCalibItem.dImageY[0], curCalibItem.dImageX[0], 1.0, out resultX, out resultY);

                HOperatorSet.ImagePointsToWorldPlane(cameraParam, finalPose, CalibPara.StartImageHeight / 2.0, CalibPara.StartImageWidth / 2.0, 1.0, out resultX, out resultY);
                if (CalibPara.CamPos == CamPos.XY)
                {
                    HOperatorSet.SetOriginPose(finalPose, resultX, resultY, 0.0, out finalPoseF);

                    CalibPara.FianlPosX = finalPoseF[0].D;
                    CalibPara.FianlPosY = finalPoseF[1].D;
                    CalibPara.FianlPosZ = finalPoseF[2].D;
                    CalibPara.FianlPosU = finalPoseF[3].D;
                    CalibPara.FianlPosV = finalPoseF[4].D;
                    CalibPara.FianlPosW = finalPoseF[5].D;
                }

                return true;
            }
            catch (Exception ex)
            {
                //MainModule.FrmManagement.frmAlarm.InsertAlarmMessage("标定失败");
                MessageBox.Show(ex.Message);
                return false;
            }
        }

        private bool CalibActionerWithHandEyeDate()
        {
            

            double dWorldX = 0.0;
            double dWorldY = 0.0;
            if (CalibPara.CamPos == CamPos.XY)
            {

            }
            else
            {


                dWorldX = 0;
                dWorldY = 0;
                double dResultX1, dResultY1;
                double dResultX2, dResultY2;

                this.TramImageToWorld(dImageAngleX[0], dImageAngleY[0], dWorldX, dWorldY, out dResultX1, out dResultY1);
                this.TramImageToWorld(dImageAngleX[1], dImageAngleY[1], dWorldX, dWorldY, out dResultX2, out dResultY2);
                double dDiffXPos1 = dTableAngleX[0] - dTableAngleX[1];
                double dDiffYPos1 = dTableAngleY[0] - dTableAngleY[1];

                double dDiffXPos2 = dResultX2 - dResultX1;
                double dDiffYPos2 = dResultY2 - dResultY1;

                double dDiffXPos = dDiffXPos1 + dDiffXPos2;
                double dDiffYPos = dDiffYPos1 + dDiffYPos2;

                double dDisTansFull = Math.Sqrt(dDiffXPos * dDiffXPos + dDiffYPos * dDiffYPos);
                double dDis = dDisTansFull / 2;
                double dAngleTemp = (dTableAngle[1] - dTableAngle[0]) / 2.0;
                double dAnglePI = dAngleTemp * Math.PI / 180.0;
                double dSin = Math.Sin(dAnglePI);
                double R = dDis / Math.Sin(dAnglePI);


                double tanAngle1 = Math.Atan2(dDiffYPos, dDiffXPos);
                double tanAngle11 = tanAngle1 * 180.0 / Math.PI;
                double dDiffAngle = (dTableAngle[1] - dTableAngle[0]);
                double dDiffTemp = 0.0;
                if (dDiffAngle > 0)
                {
                    dDiffTemp = 90 - dDiffAngle / 2.0;
                }
                else
                {
                    dDiffTemp = 90 - dDiffAngle / 2.0;
                }
                double dAngleActly = tanAngle11 + dDiffTemp - 180 - dTableAngle[0];
                double dAngleActly1 = dAngleActly + dTableAngle[0];
                double dOffX = R * Math.Cos(dAngleActly1 * Math.PI / 180.0);
                double dOffY = R * Math.Sin(dAngleActly1 * Math.PI / 180.0);
                for (int i = 0; i < 9; i++)
                {
                    dTableXCalib[i] = dTableXCalib[i] + dOffX;
                    dTableYCalib[i] = dTableYCalib[i] + dOffY;

                }
                CalibActioner();
            }

            return true;
        }
    }
}
